#include "AStar.h"
#include <set>
#include <queue>
#include "Log.h"

using namespace std;

#define MAX_ASTAR_NODES 2500

template <typename T>
struct AStarNode
{
//	AStarNode(const AStarNode&);
	AStarNode& operator=(const AStarNode&);
public:
	AStarNode *parent;
	T node;
	
	double cost;			// Actual cost to get to n so far
	double estimated_cost;	// Actual cost to get to n so far + estimated cost to the end
	
	void set(AStarNode *p, T n, double c, double e) {
		parent = p;	node = n;	cost = c;	estimated_cost = e;
	}
	
	AStarNode(AStarNode *p = NULL, T n = T(), double c = -1, double e = -1) :
		parent(p), node(n), cost(c), estimated_cost(e) { }

	bool operator<(const AStarNode &n) const {
		return n.estimated_cost < this->estimated_cost;
	}
	
	bool operator==(const AStarNode &o) const {
		return this->node == o.node;
	}
};

template<typename T>
struct AStarNodePointerLessThan {
	bool operator()(const AStarNode<T> *a, const AStarNode<T> *b) const {
		return (*a) < (*b);
	}
};

template <typename T>
vector <T> a_star(const SearchableMap<T> &m, const T &start, const T &end)
{
	vector <T> path;
	priority_queue < AStarNode<T>* , vector<AStarNode<T>*>, AStarNodePointerLessThan<T> > nodes_to_expand;
	set <T> nodes_visited;
	typename set <T>::iterator it;		// For testing node membership of node_visted
	
	//All nodes to be used so we can shuffle around pointers SAFELY
	AStarNode<T> nodes[MAX_ASTAR_NODES];
	unsigned nodeIndex = 0;
	
//	lout << "start = "<<start.x<<","<<start.y<<","<<endl;
//	lout << "end = "<<end.x<<","<<end.y<<","<<endl;
	
	//Push first node
	AStarNode <T>* Start = &nodes[nodeIndex++];
	Start->set(NULL, start, m.get_estimated_distance(start, end), 0);
	nodes_to_expand.push(Start);

//	lout<<"Pushed first node"<<endl;
	while (!nodes_to_expand.empty())
	{
		AStarNode <T>* n = nodes_to_expand.top();
		nodes_to_expand.pop();
		
//		lout<<"Expanding "<<n->node.x<<","<<n->node.y<<","<<endl;
	
		if (n->node == end)		// Found the goal
		{
			AStarNode<T>* p = n;
			for (; p ; p = p->parent) {
				path.insert(path.begin(), p->node);
			}
               
			break;
		}
		else	// Expand this node
		{
//			lout<<"Wasn't the goal"<<endl;
			vector< pair<T, double> > neighbors = m.get_adjacent_positions(n->node);
			
//			lout<<"Got "<<neighbors.size()<<" neighbors"<<endl;
			for (unsigned int i = 0; i < neighbors.size(); i ++)
			{
				T new_node = neighbors[i].first;
//				lout<<"Examining "<<new_node.x<<","<<new_node.y<<endl;
//				lout<<"Visited size = "<<nodes_visited.size()<<endl;
				it = nodes_visited.find(new_node);
				if (it == nodes_visited.end())	// We haven't visited this node
				{
					double cost = n->cost + neighbors[i].second;
					double estimated_cost = cost + m.get_estimated_distance(new_node, end);
//					lout<<i<<": Haven't visited, cost = "<<cost<<" estimated = "<<estimated_cost<<" pushing onto queue"<<endl;
					
					if(nodeIndex >= MAX_ASTAR_NODES) {
						lout<<"WARNING - AStar: Out of nodes..."<<endl;
						return path;
					}
					AStarNode<T>* newANode = &nodes[nodeIndex++];
					newANode->set(n, new_node, cost, estimated_cost);
					nodes_to_expand.push(newANode);
					nodes_visited.insert(new_node);
//					lout<<"Visited size = "<<nodes_visited.size()<<endl;
				} else {
//					lout<<i<<": Already visited "<<it->x<<","<<it->y<<"  "<<(void*)&(*it)<<endl;
				}
			}
		}
		
		nodes_visited.insert(n->node);
//		lout<<"Visited size = "<<nodes_visited.size()<<endl;
	}
	
	return path;
}

//TODO		FIX THIS! Why is this necessary when Unit.cpp has a reference to a_star? WTF?
#include "../game/Map.h"
void foo()
{
	Map map;
	a_star(map, lVec2D(0,0), lVec2D(1,1));	
}
